#include <system.h>


void XMOTOR_init(void) {
	
	DDRB |= (1<<PB0)|(1<<PB1)|(1<<PB2)|(1<<PB3);
	DDRD |= (1<<PD4)|(1<<PD5);
	
	PORTB &= ~((1<<PB0)|(1<<PB1)|(1<<PB2)|(1<<PB3));
	PORTD &= ~((1<<PD4)|(1<<PD5));
	
	/* Enable pin PD4 and pin PD5 as the PWM output pins.
	   Set the hardware timer mode to phase correct-PWM 10-bit mode. */
	TCCR1A = (1<<COM1A1)|(1<<COM1B1)|(1<<WGM11)|(1<<WGM10);
	
	/* set 0% duty cycle */
	OCR1A = 0;
	OCR1B = 0;
	
	/* set prescaler */
	TCCR1B = (1<<CS11)|(1<<CS10);
}


void XMOTOR_setDirection(uint8_t side, uint8_t value) {
	
	if (side == XMOTOR_RIGHT) {		//left motor dir control
		
		if (value == XMOTOR_FORWARD) {
			CLR_BIT(PORTB, PB0);
			SET_BIT(PORTB, PB1);
		}
		else if (value == XMOTOR_BACKWARD) {
			SET_BIT(PORTB, PB0);
			CLR_BIT(PORTB, PB1);		
		}
		else {
			CLR_BIT(PORTB, PB0);
			CLR_BIT(PORTB, PB1);		
		}
		
	}
	
	else if (side == XMOTOR_LEFT) {						//right motor dir control
	
		if(value == XMOTOR_FORWARD) {
			SET_BIT(PORTB, PB2);
			CLR_BIT(PORTB, PB3);
		}
		else if(value == XMOTOR_BACKWARD) {
			SET_BIT(PORTB, PB3);
			CLR_BIT(PORTB, PB2);		
		}	
		else {
			CLR_BIT(PORTB, PB2);
			CLR_BIT(PORTB, PB3);		
		}
	
	}
}


void XMOTOR_setSpeed(uint8_t side, uint16_t value) {
	
	value &= 0x3ff;
	
	if (side == XMOTOR_LEFT)
		OCR1A = value;
	else if (side == XMOTOR_RIGHT)
		OCR1B = value;
}
